#include "PandaConv.h"

// Constructor
PandaConv::PandaConv(UINT32 MotorChannelright, UINT32 MotorChannelleft, UINT32 MotorSlotmiddle, UINT32 MotorChannelmiddle, Joystick* joystick, int leftConvOffButton, int rightConvOffButton) 
{
	m_leftConvOffButton = leftConvOffButton;
	m_rightConvOffButton = rightConvOffButton;
	m_motorright = new Jaguar(MotorChannelright);
	m_motorleft = new Jaguar (MotorChannelleft);
	m_motormiddle = new Relay (MotorSlotmiddle, MotorChannelmiddle);
	m_joystick = joystick;	
	leftReset = true;
	rightReset = true;
	leftOff = false;
	rightOff = false;
}

// Destructor
PandaConv::~PandaConv(void)
{
	delete m_motorright;
	delete m_motorleft;
	delete m_motormiddle;
}

int PandaConv::teleop()
{
	int retval = 0;
	if (m_joystick->GetRawButton(m_leftConvOffButton))
		{
			if (leftReset)
			{
				if (leftOff)
					leftOff = false;
				else
					leftOff = true;
				leftReset = false;
			}
		} else
			leftReset = true;
	if (m_joystick->GetRawButton(m_rightConvOffButton))
		{
			if (rightReset)
			{
				if (rightOff)
					rightOff = false;
				else
					rightOff = true;
				rightReset = false;
			}
		} else
			rightReset = true;
	if (!leftOff) retval += 1;
	if (!rightOff) retval += 2;
	float Z = m_joystick->GetZ();
		if (Z > 0.2)
		{
			if (rightOff == false)
			m_motorright->Set(-(Z-0.2)/0.8);//linear scaling
			else
			m_motorright->Set(0);
			if (leftOff == false)
			m_motorleft->Set((Z-0.2)/0.8);//more linear scaling
			else
			m_motorleft->Set(0);
			m_motormiddle->Set(Relay::kForward);
		}
		else if (Z < -0.2)
		{
			if (rightOff == false)
			m_motorright->Set(-(Z+0.2)/0.8);
			else
			m_motorright->Set(0);
			if (leftOff == false)
			m_motorleft->Set((Z+0.2)/0.8);
			else
			m_motorleft->Set(0);
			m_motormiddle->Set(Relay::kReverse);
		}
		else
		{
			m_motorright->Set(0);
			m_motorleft->Set(0);
			m_motormiddle->Set(Relay::kOff);
		}
	return retval;
}

void PandaConv::Drive(float motorSpeed)
{
	m_motorright->Set(motorSpeed);
	m_motorleft->Set(-motorSpeed);
	if(motorSpeed < 0.2)
		m_motormiddle->Set(Relay::kOff);
	else
		m_motormiddle->Set(Relay::kForward);
}

